#include <laser_scan_mapping/laser_scan_mapping.h>

namespace laser_scan_graph_slam
{

LaserScanMapping::LaserScanMapping(h, h_private):
  nh_(nh),
  nh_private_(nh_private)
{
  RCLCPP_INFO(node->get_logger(),"Starting LaserScanMapping");

  // **** init parameters

  initParams();
}

LaserScanMapping::~LaserScanMapping()
{
  RCLCPP_INFO(node->get_logger(),"Destroying LaserScanMapping");
}

void LaserScanMapping::initParams()
{
  graph_slam.reset(new GraphSLAM(nh_private_.param<std::string>("g2o_solver_type", "lm_var")));
  keyframe_updater.reset(new KeyframeUpdater(nh_private_));
  loop_detector.reset(new LoopDetector(nh_private_));
  map_cloud_generator.reset(new MapCloudGenerator());
  inf_calclator.reset(new InformationMatrixCalculator(nh_private_));
  tf_.reset(new tf2_ros::Buffer());
  tfl_.reset(new tf2_ros::TransformListener(*tf_));
  trans_odom2map.setIdentity();

  map_frame_id = nh_private_.param<std::string>("map_frame_id", "map");
  odom_frame_id = nh_private_.param<std::string>("odom_frame_id", "odom");
  robot_frame_id = nh_private_.param<std::string>("robot_frame_id", "base_link");
  map_cloud_resolution = nh_private_.param<double>("map_cloud_resolution", 0.05);
  if (nh_private_.param<double>("occupancy_grid_resolution", 0.05) < map_cloud_resolution)
  {
    map_cloud_resolution = nh_private_.param<double>("occupancy_grid_resolution", 0.05);
  }
  
  max_keyframes_per_update = nh_private_.param<int>("max_keyframes_per_update", 10);

  use_distance_filter = nh_private_.param<bool>("use_distance_filter", true);
  distance_near_thresh = nh_private_.param<double>("distance_near_thresh", 0.01);
  distance_far_thresh = nh_private_.param<double>("distance_far_thresh", 20.0);
  distance_rear_thresh = nh_private_.param<double>("distance_rear_thresh", 5.0);


  geometry_msgs::msg::TransformStamped ts;
  ts.header.stamp = rclcpp::Clock().now();
  ts.header.frame_id = map_frame_id;
  ts.child_frame_id = odom_frame_id;
  ts.transform.rotation.w = 1.0;
  static_tf_broadcaster_.sendTransform(ts);

  anchor_node = nullptr;

  scan_cloud_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>("/debug/scan_cloud", 5);
  base_scan_cloud_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>("/debug/base_scan_cloud", 5);
  map_points_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>("/debug/map_points", 1, true);
  map_grid_pub_ = node->create_publisher<nav_msgs::msg::OccupancyGrid>("/map", 1, true);
  markers_pub_ = node->create_publisher<visualization_msgs::msg::MarkerArray>("/debug/markers", 16);
  // debug_scan_pub_ = node->create_publisher<sensor_msgs::msg::LaserScan>("/debug/scan", 5);

  save_map_service_server = node->create_publisherService("/laser_scan_mapping/save_map", &LaserScanMapping::save_map_service, this);

  odom_sub.reset(new message_filters::Subscriber<nav_msgs::msg::Odometry>(nh_private_, "/laser_odom", 256));
  scan_sub.reset(new message_filters::Subscriber<sensor_msgs::msg::LaserScan>(nh_private_, "/scan", 32));
  sync.reset(new message_filters::Synchronizer<ApproxSyncPolicy>(ApproxSyncPolicy(32), *odom_sub, *scan_sub));
  sync->registerCallback(boost::bind(std::bind(&LaserScanMapping::scanCallback, this, std::placeholders::_1));

  graph_updated = false;
  double graph_update_interval = nh_private_.param<double>("graph_update_interval", 3.0);
  double map_cloud_update_interval = nh_private_.param<double>("map_cloud_update_interval", 10.0);
  optimization_timer = nh_.createWallTimer(ros::WallDuration(graph_update_interval), std::bind(&LaserScanMapping::optimization_timer_callback, this, std::placeholders::_1));
  map_publish_timer = nh_.createWallTimer(ros::WallDuration(map_cloud_update_interval), std::bind(&LaserScanMapping::map_points_publish_timer_callback, this, std::placeholders::_1));
}

void LaserScanMapping::scanCallback (const nav_msgs::msg::OdometryConstPtr& odom_msg, const sensor_msgs::msg::LaserScan& scan_msg)
{
  // printf("[debug] scanCallback \n");
  Eigen::Isometry3d odom = odom2isometry(odom_msg);
  const rclcpp::Time& stamp = scan_msg->header.stamp;
  //激光转点云
  sensor_msgs::msg::PointCloud2 cloud_msg;
  projector_.projectLaser(*scan_msg, cloud_msg);

  // 激光转到base_link
  if (!init_sensor_to_base)
  {
    try{
      base_to_senser_trans = tf_->lookupTransform(robot_frame_id, cloud_msg.header.frame_id,rclcpp::Time(0));
      std::cout << base_to_senser_trans <<std::endl;
      base_to_senser_mat = tf2::transformToEigen(base_to_senser_trans.transform).matrix().cast<float>();
      init_sensor_to_base = true;
    } catch (tf2::TransformException &ex) {
      RCLCPP_WARN(node->get_logger(),"%s", ex.what());
      return;
    }
  }

  sensor_msgs::msg::PointCloud2 cloud_msg_new;
  tf2::doTransform(cloud_msg, cloud_msg_new, base_to_senser_trans);
  scan_cloud_pub_->publish(cloud_msg);
  base_scan_cloud_pub_->publish(cloud_msg_new);

  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
  pcl::fromROSMsg(cloud_msg_new, *cloud);

  pcl::PointCloud<PointT> filtered;
  filtered = cloud;
  if (use_distance_filter)
  {
    filtered = distance_filter(filtered);
  }

  if(!keyframe_updater->update(odom)) 
  {
    //非关键帧放入临时队列
    if (tmp_frame_queue.empty())
    {
      // RCLCPP_WARN(node->get_logger(),"tmp_frame_queue.empty");
      Frame tmp_frame;
      tmp_frame.cloud = *filtered;
      tmp_frame.odom = odom;
      tmp_frame_queue.push_back(tmp_frame);
    }else
    {
      //检查距离是否需要放入队列
      Eigen::Isometry3d delta = tmp_frame_queue.back().odom.inverse() * odom;
      double dx = delta.translation().norm();
      double da = Eigen::AngleAxisd(delta.linear()).angle();
      if (dx > 5.0 || da > 0.785)
      {
        Frame tmp_frame;
        tmp_frame.cloud = *filtered;
        tmp_frame.odom = odom;
        tmp_frame_queue.push_back(tmp_frame);
      }
    }
    std::lock_guard<std::mutex> lock(keyframe_queue_mutex);
    if(keyframe_queue.empty()) {
      // std_msgs::msg::Header read_until;
      // read_until.stamp = stamp + ros::Duration(10, 0);
      // read_until.frame_id = "/filtered_points";
    }
    return;
  }
  printf("tmp_frame_queue size %ld \n", tmp_frame_queue.size());

  pcl::PointCloud<PointT>::Ptr cloud_final{ new pcl::PointCloud<PointT> };
  
  double accum_d = keyframe_updater->get_accum_distance();

  for (auto it = tmp_frame_queue.begin(); it != tmp_frame_queue.end(); ++it) {
    Eigen::Isometry3d pre_pose = it->odom;
    Eigen::Isometry3d relative_pose = odom.inverse() * pre_pose;
    Eigen::Matrix4f relative_mat = relative_pose.matrix().cast<float>();
    pcl::PointCloud<PointT>::Ptr pre_cloud(new pcl::PointCloud<PointT>());
    pcl::transformPointCloud(it->cloud, *pre_cloud, relative_mat);
    *cloud_final += *pre_cloud;
  }
  tmp_frame_queue.clear();
  // if (pre_keyframe != nullptr)
  // {
  //   Eigen::Isometry3d relative_pose = odom.inverse() * pre_keyframe->odom;
  //   Eigen::Matrix4f relative_mat = relative_pose.matrix().cast<float>();
  //   pcl::PointCloud<PointT>::Ptr pre_cloud(new pcl::PointCloud<PointT>());
  //   pcl::transformPointCloud(*pre_keyframe->cloud, *pre_cloud, relative_mat);
  //   // pcl::PointCloud<PointT>::Ptr cloud_final{ new pcl::PointCloud<PointT> };
  //   pcl::CropBox<PointT> clipper;
  //   clipper.setMin(Eigen::Vector4f(-distance_rear_thresh,-distance_rear_thresh,-1.0,1.));
  //   clipper.setMax(Eigen::Vector4f(0.0,distance_rear_thresh,1.0,1.));
  //   clipper.setInputCloud(pre_cloud);
  //   clipper.filter(*cloud_final);
  //   clipper.setNegative(false);//默认为false
  // }
  *cloud_final += *filtered;

  printf("before size %ld \n", cloud_final->size());
  pcl::PointCloud<PointT>::Ptr final_filtered(new pcl::PointCloud<PointT>);//输出点云
	pcl::VoxelGrid<PointT> sor; //创建滤波器
	sor.setInputCloud(cloud_final);//待处理点云cloud放进去
	sor.setLeafSize(map_cloud_resolution, map_cloud_resolution, map_cloud_resolution);//三维格网或者叫体素格网的长宽高
	sor.filter(*final_filtered);//接受滤波处理后的点云容器放进去


  printf("after size %ld \n\n", final_filtered->size());
  KeyFrame::Ptr keyframe(new KeyFrame(stamp, odom, accum_d, final_filtered));
  std::lock_guard<std::mutex> lock(keyframe_queue_mutex);
  keyframe_queue.push_back(keyframe);
  pre_keyframe = keyframe;
}

/**
 * @description: this method adds all the keyframes in #keyframe_queue to the pose graph (odometry edges)     
 * @param:  
 * @return:   
 */
bool LaserScanMapping::flush_keyframe_queue() 
{
  // printf("[debug] flush_keyframe_queue \n");
  std::lock_guard<std::mutex> lock(keyframe_queue_mutex);
  if(keyframe_queue.empty()) {
    return false;
  }

  trans_odom2map_mutex.lock();
  Eigen::Isometry3d odom2map(trans_odom2map.cast<double>());
  trans_odom2map_mutex.unlock();

  int num_processed = 0;
  for(int i = 0; i < std::min<int>(keyframe_queue.size(), max_keyframes_per_update); i++) {
    num_processed = i;

    const auto& keyframe = keyframe_queue[i];
    // new_keyframes will be tested later for loop closure
    new_keyframes.push_back(keyframe);

    // add pose node
    Eigen::Isometry3d odom = odom2map * keyframe->odom;
    keyframe->node = graph_slam->add_se3_node(odom);
    keyframe_hash[keyframe->stamp] = keyframe;

    // fix the first node
    if(keyframes.empty() && new_keyframes.size() == 1) {
      if(nh_private_.param<bool>("fix_first_node", false)) {
        Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6);
        std::stringstream sst(nh_private_.param<std::string>("fix_first_node_stddev", "1 1 1 1 1 1"));
        for(int i = 0; i < 6; i++) {
          double stddev = 1.0;
          sst >> stddev;
          inf(i, i) = 1.0 / stddev;
        }
        anchor_node = graph_slam->add_se3_node(Eigen::Isometry3d::Identity());
        anchor_node->setFixed(true);
        anchor_edge = graph_slam->add_se3_edge(anchor_node, keyframe->node, Eigen::Isometry3d::Identity(), inf);
      }
    }

    if(i == 0 && keyframes.empty()) {
      continue;
    }

    // add edge between consecutive keyframes
    const auto& prev_keyframe = i == 0 ? keyframes.back() : keyframe_queue[i - 1];

    Eigen::Isometry3d relative_pose = keyframe->odom.inverse() * prev_keyframe->odom;
    Eigen::MatrixXd information = inf_calclator->calc_information_matrix(keyframe->cloud, prev_keyframe->cloud, relative_pose);
    auto edge = graph_slam->add_se3_edge(keyframe->node, prev_keyframe->node, relative_pose, information);
    graph_slam->add_robust_kernel(edge, nh_private_.param<std::string>("odometry_edge_robust_kernel", "NONE"), nh_private_.param<double>("odometry_edge_robust_kernel_size", 1.0));
  }

  // std_msgs::msg::Header read_until;
  // read_until.stamp = keyframe_queue[num_processed]->stamp + ros::Duration(10, 0);
  // read_until.frame_id = points_topic;
  // read_until_pub->publish(read_until);
  // read_until.frame_id = "/filtered_points";
  // read_until_pub->publish(read_until);

  keyframe_queue.erase(keyframe_queue.begin(), keyframe_queue.begin() + num_processed + 1);
  return true;
}

/**
 * @description: this methods adds all the data in the queues to the pose graph, and then optimizes the pose graph
 * @param:  
 * @return:   
 */
  void LaserScanMapping::optimization_timer_callback(const ros::WallTimerEvent::SharedPtr event) {
    // printf("[debug] optimization_timer_callback \n");
    std::lock_guard<std::mutex> lock(main_thread_mutex);

    // add keyframes in the queues to the pose graph
    bool keyframe_updated = flush_keyframe_queue();

    if(!keyframe_updated) {
      // std_msgs::msg::Header read_until;
      // read_until.stamp = rclcpp::Clock().now() + ros::Duration(30, 0);
      // read_until.frame_id = points_topic;
      // read_until_pub->publish(read_until);
      // read_until.frame_id = "/filtered_points";
      // read_until_pub->publish(read_until);
    }

    if(!keyframe_updated) {
      return;
    }

    // loop detection
    std::vector<Loop::Ptr> loops = loop_detector->detect(keyframes, new_keyframes, *graph_slam);
    for(const auto& loop : loops) {
      // geometry_msgs::msg::Pose pose;
      // pose.orientation.w = 1.0;
      // Eigen::Isometry3d relpose = pose2isometry(pose);
      // std::cout << "[loop relpose]" << pose << std::endl;
      Eigen::Isometry3d relpose(loop->relative_pose.cast<double>());
      Eigen::MatrixXd information_matrix = inf_calclator->calc_information_matrix(loop->key1->cloud, loop->key2->cloud, relpose);
      auto edge = graph_slam->add_se3_edge(loop->key1->node, loop->key2->node, relpose, information_matrix);
      graph_slam->add_robust_kernel(edge, nh_private_.param<std::string>("loop_closure_edge_robust_kernel", "NONE"), nh_private_.param<double>("loop_closure_edge_robust_kernel_size", 1.0));
    }
    std::copy(new_keyframes.begin(), new_keyframes.end(), std::back_inserter(keyframes));
    new_keyframes.clear();
    // move the first node anchor position to the current estimate of the first node pose
    // so the first node moves freely while trying to stay around the origin
    if(anchor_node && nh_private_.param<bool>("fix_first_node_adaptive", true)) {
      Eigen::Isometry3d anchor_target = static_cast<g2o::VertexSE3*>(anchor_edge->vertices()[1])->estimate();
      anchor_node->setEstimate(anchor_target);
    }

    // // optimize the pose graph
    int num_iterations = nh_private_.param<int>("g2o_solver_num_iterations", 1024);
    graph_slam->optimize(num_iterations);
    // publish tf
    const auto& keyframe = keyframes.back();
    Eigen::Isometry3d trans = keyframe->node->estimate() * keyframe->odom.inverse();
    trans_odom2map_mutex.lock();
    trans_odom2map = trans.matrix().cast<float>();
    trans_odom2map_mutex.unlock();

    std::vector<KeyFrameSnapshot::Ptr> snapshot(keyframes.size());
    std::transform(keyframes.begin(), keyframes.end(), snapshot.begin(), [=](const KeyFrame::Ptr& k) { return std::make_shared<KeyFrameSnapshot>(k); });
    keyframes_snapshot_mutex.lock();
    keyframes_snapshot.swap(snapshot);
    keyframes_snapshot_mutex.unlock();
    graph_updated = true;

    // if(odom2map_pub.getNumSubscribers()) {
      geometry_msgs::msg::TransformStamped ts = matrix2transform(keyframe->stamp, trans.matrix().cast<float>(), map_frame_id, odom_frame_id);
      static_tf_broadcaster_.sendTransform(ts);
    // }
    if(markers_pub_.getNumSubscribers()) {
      auto markers = create_marker_array(rclcpp::Clock().now());
      markers_pub_->publish(markers);
    }
  }

    /**
   * @brief create visualization marker
   * @param stamp
   * @return
   */
  visualization_msgs::msg::MarkerArray LaserScanMapping::create_marker_array(const rclcpp::Time& stamp) const {
    visualization_msgs::msg::MarkerArray markers;
    markers.markers.resize(4);

    // node markers
    visualization_msgs::msg::Marker& traj_marker = markers.markers[0];
    traj_marker.header.frame_id = "map";
    traj_marker.header.stamp = stamp;
    traj_marker.ns = "nodes";
    traj_marker.id = 0;
    traj_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST;

    traj_marker.pose.orientation.w = 1.0;
    traj_marker.scale.x = traj_marker.scale.y = traj_marker.scale.z = 0.5;

    visualization_msgs::msg::Marker& imu_marker = markers.markers[1];
    imu_marker.header = traj_marker.header;
    imu_marker.ns = "imu";
    imu_marker.id = 1;
    imu_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST;

    imu_marker.pose.orientation.w = 1.0;
    imu_marker.scale.x = imu_marker.scale.y = imu_marker.scale.z = 0.75;

    traj_marker.points.resize(keyframes.size());
    traj_marker.colors.resize(keyframes.size());
    for(int i = 0; i < keyframes.size(); i++) {
      Eigen::Vector3d pos = keyframes[i]->node->estimate().translation();
      traj_marker.points[i].x = pos.x();
      traj_marker.points[i].y = pos.y();
      traj_marker.points[i].z = pos.z();

      double p = static_cast<double>(i) / keyframes.size();
      traj_marker.colors[i].r = 1.0 - p;
      traj_marker.colors[i].g = p;
      traj_marker.colors[i].b = 0.0;
      traj_marker.colors[i].a = 1.0;

      if(keyframes[i]->acceleration) {
        Eigen::Vector3d pos = keyframes[i]->node->estimate().translation();
        geometry_msgs::msg::Point point;
        point.x = pos.x();
        point.y = pos.y();
        point.z = pos.z();

        std_msgs::msg::ColorRGBA color;
        color.r = 0.0;
        color.g = 0.0;
        color.b = 1.0;
        color.a = 0.1;

        imu_marker.points.push_back(point);
        imu_marker.colors.push_back(color);
      }
    }

    // edge markers
    visualization_msgs::msg::Marker& edge_marker = markers.markers[2];
    edge_marker.header.frame_id = "map";
    edge_marker.header.stamp = stamp;
    edge_marker.ns = "edges";
    edge_marker.id = 2;
    edge_marker.type = visualization_msgs::msg::Marker::LINE_LIST;

    edge_marker.pose.orientation.w = 1.0;
    edge_marker.scale.x = 0.05;

    edge_marker.points.resize(graph_slam->graph->edges().size() * 2);
    edge_marker.colors.resize(graph_slam->graph->edges().size() * 2);

    auto edge_itr = graph_slam->graph->edges().begin();
    for(int i = 0; edge_itr != graph_slam->graph->edges().end(); edge_itr++, i++) {
      g2o::HyperGraph::Edge* edge = *edge_itr;
      g2o::EdgeSE3* edge_se3 = dynamic_cast<g2o::EdgeSE3*>(edge);
      if(edge_se3) {
        g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_se3->vertices()[0]);
        g2o::VertexSE3* v2 = dynamic_cast<g2o::VertexSE3*>(edge_se3->vertices()[1]);
        Eigen::Vector3d pt1 = v1->estimate().translation();
        Eigen::Vector3d pt2 = v2->estimate().translation();

        edge_marker.points[i * 2].x = pt1.x();
        edge_marker.points[i * 2].y = pt1.y();
        edge_marker.points[i * 2].z = pt1.z();
        edge_marker.points[i * 2 + 1].x = pt2.x();
        edge_marker.points[i * 2 + 1].y = pt2.y();
        edge_marker.points[i * 2 + 1].z = pt2.z();

        double p1 = static_cast<double>(v1->id()) / graph_slam->graph->vertices().size();
        double p2 = static_cast<double>(v2->id()) / graph_slam->graph->vertices().size();
        edge_marker.colors[i * 2].r = 1.0 - p1;
        edge_marker.colors[i * 2].g = p1;
        edge_marker.colors[i * 2].a = 1.0;
        edge_marker.colors[i * 2 + 1].r = 1.0 - p2;
        edge_marker.colors[i * 2 + 1].g = p2;
        edge_marker.colors[i * 2 + 1].a = 1.0;

        if(std::abs(v1->id() - v2->id()) > 2) {
          edge_marker.points[i * 2].z += 0.5;
          edge_marker.points[i * 2 + 1].z += 0.5;
        }

        continue;
      }

      g2o::EdgeSE3Plane* edge_plane = dynamic_cast<g2o::EdgeSE3Plane*>(edge);
      if(edge_plane) {
        g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_plane->vertices()[0]);
        Eigen::Vector3d pt1 = v1->estimate().translation();
        Eigen::Vector3d pt2(pt1.x(), pt1.y(), 0.0);

        edge_marker.points[i * 2].x = pt1.x();
        edge_marker.points[i * 2].y = pt1.y();
        edge_marker.points[i * 2].z = pt1.z();
        edge_marker.points[i * 2 + 1].x = pt2.x();
        edge_marker.points[i * 2 + 1].y = pt2.y();
        edge_marker.points[i * 2 + 1].z = pt2.z();

        edge_marker.colors[i * 2].b = 1.0;
        edge_marker.colors[i * 2].a = 1.0;
        edge_marker.colors[i * 2 + 1].b = 1.0;
        edge_marker.colors[i * 2 + 1].a = 1.0;

        continue;
      }

      g2o::EdgeSE3PriorXY* edge_priori_xy = dynamic_cast<g2o::EdgeSE3PriorXY*>(edge);
      if(edge_priori_xy) {
        g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_priori_xy->vertices()[0]);
        Eigen::Vector3d pt1 = v1->estimate().translation();
        Eigen::Vector3d pt2 = Eigen::Vector3d::Zero();
        pt2.head<2>() = edge_priori_xy->measurement();

        edge_marker.points[i * 2].x = pt1.x();
        edge_marker.points[i * 2].y = pt1.y();
        edge_marker.points[i * 2].z = pt1.z() + 0.5;
        edge_marker.points[i * 2 + 1].x = pt2.x();
        edge_marker.points[i * 2 + 1].y = pt2.y();
        edge_marker.points[i * 2 + 1].z = pt2.z() + 0.5;

        edge_marker.colors[i * 2].r = 1.0;
        edge_marker.colors[i * 2].a = 1.0;
        edge_marker.colors[i * 2 + 1].r = 1.0;
        edge_marker.colors[i * 2 + 1].a = 1.0;

        continue;
      }

      g2o::EdgeSE3PriorXYZ* edge_priori_xyz = dynamic_cast<g2o::EdgeSE3PriorXYZ*>(edge);
      if(edge_priori_xyz) {
        g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_priori_xyz->vertices()[0]);
        Eigen::Vector3d pt1 = v1->estimate().translation();
        Eigen::Vector3d pt2 = edge_priori_xyz->measurement();

        edge_marker.points[i * 2].x = pt1.x();
        edge_marker.points[i * 2].y = pt1.y();
        edge_marker.points[i * 2].z = pt1.z() + 0.5;
        edge_marker.points[i * 2 + 1].x = pt2.x();
        edge_marker.points[i * 2 + 1].y = pt2.y();
        edge_marker.points[i * 2 + 1].z = pt2.z();

        edge_marker.colors[i * 2].r = 1.0;
        edge_marker.colors[i * 2].a = 1.0;
        edge_marker.colors[i * 2 + 1].r = 1.0;
        edge_marker.colors[i * 2 + 1].a = 1.0;

        continue;
      }
    }

    // sphere
    visualization_msgs::msg::Marker& sphere_marker = markers.markers[3];
    sphere_marker.header.frame_id = "map";
    sphere_marker.header.stamp = stamp;
    sphere_marker.ns = "loop_close_radius";
    sphere_marker.id = 3;
    sphere_marker.type = visualization_msgs::msg::Marker::SPHERE;

    if(!keyframes.empty()) {
      Eigen::Vector3d pos = keyframes.back()->node->estimate().translation();
      sphere_marker.pose.position.x = pos.x();
      sphere_marker.pose.position.y = pos.y();
      sphere_marker.pose.position.z = pos.z();
    }
    sphere_marker.pose.orientation.w = 1.0;
    sphere_marker.scale.x = sphere_marker.scale.y = sphere_marker.scale.z = loop_detector->get_distance_thresh() * 2.0;

    sphere_marker.color.r = 1.0;
    sphere_marker.color.a = 0.3;

    return markers;
  }

  /**
   * @description: generate map point cloud and publish it
   * @param:  
   * @return:   
   */
  void LaserScanMapping::map_points_publish_timer_callback(const ros::WallTimerEvent::SharedPtr event)
  {
    // printf("[debug] map_points_publish_timer_callback \n");
    if(!map_points_pub_.getNumSubscribers() && !map_grid_pub_.getNumSubscribers()) {
      return;
    }

    std::vector<KeyFrameSnapshot::Ptr> snapshot;

    keyframes_snapshot_mutex.lock();
    snapshot = keyframes_snapshot;
    keyframes_snapshot_mutex.unlock();

    
    //======================== 合并点云 ================================
    // {
    //   pcl::PointCloud<PointT>::Ptr map_cloud(new pcl::PointCloud<PointT>());
    //   for (int i = 0; i < keyframe_queue.size(); i++)
    //   {
    //     const auto& keyframe = keyframe_queue[i];
    //     pcl::PointCloud<PointT> cloud = keyframe->cloud;
    //     Eigen::Isometry3d odom = keyframe->odom;

    //     pcl::PointCloud<PointT>::Ptr tmp(new pcl::PointCloud<PointT>());
    //     pcl::transformPointCloud( *cloud, *tmp, odom.matrix());
    //     *map_cloud += *tmp;
    //   }

    //   sensor_msgs::msg::PointCloud2Ptr map_cloud_msg(new sensor_msgs::msg::PointCloud2());
    //   pcl::toROSMsg(*map_cloud, *map_cloud_msg);
    //   map_cloud_msg->header.frame_id = "odom";
    //   map_cloud_msg->header.stamp = rclcpp::Clock().now();
    //   map_points_pub_->publish(map_cloud_msg);
    // }
    //========================= end ===================================
    
    auto cloud = map_cloud_generator->generate(snapshot, map_cloud_resolution);
    if(!cloud) {
      return;
    }

    cloud->header.frame_id = map_frame_id;
    cloud->header.stamp = snapshot.back()->cloud->header.stamp;
    sensor_msgs::msg::PointCloud2Ptr cloud_msg(new sensor_msgs::msg::PointCloud2());
    if (nh_private_.param<bool>("enable_filtered_map", false))
    {
      pcl::RadiusOutlierRemoval<PointT>::Ptr rad(new pcl::RadiusOutlierRemoval<PointT>());
      rad->setRadiusSearch(nh_private_.param<double>("radius_radius", 0.02));
      rad->setMinNeighborsInRadius(nh_private_.param<int>("radius_min_neighbors", 1));
      pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
      rad->setInputCloud(cloud);
      rad->filter(*filtered);
      filtered->header = cloud->header;
      printf("before size %ld after size %ld \n\n", cloud->points.size(), filtered->points.size());
      pcl::toROSMsg(*filtered, *cloud_msg); 
    }else{
      pcl::toROSMsg(*cloud, *cloud_msg); 
    }
    map_points_pub_->publish(cloud_msg);

    // double resolution = nh_private_.param<double>("occupancy_grid_resolution", 0.025);
    // cloud = map_cloud_generator->generate(snapshot, resolution / 2.0);
    pointcloud_to_occupancy_grid(snapshot, cloud);
  }

  /**
   * @brief save map data as pcd
   * @param req
   * @param res
   * @return
   */
  bool LaserScanMapping::save_map_service(laser_scan_graph_slam::SaveMapRequest& req, laser_scan_graph_slam::SaveMapResponse& res) {
    std::vector<KeyFrameSnapshot::Ptr> snapshot;

    keyframes_snapshot_mutex.lock();
    snapshot = keyframes_snapshot;
    keyframes_snapshot_mutex.unlock();

    auto cloud = map_cloud_generator->generate(snapshot, req.resolution);
    if(!cloud) {
      res.success = false;
      return true;
    }

    cloud->header.frame_id = map_frame_id;
    cloud->header.stamp = snapshot.back()->cloud->header.stamp;
    //@todo 抽出来
    int ret = 1;
    if (nh_private_.param<bool>("enable_filtered_map", false))
    {
      pcl::RadiusOutlierRemoval<PointT>::Ptr rad(new pcl::RadiusOutlierRemoval<PointT>());
      rad->setRadiusSearch(nh_private_.param<double>("radius_radius", 0.02));
      rad->setMinNeighborsInRadius(nh_private_.param<int>("radius_min_neighbors", 1));
      pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
      rad->setInputCloud(cloud);
      rad->filter(*filtered);
      filtered->header = cloud->header;
      ret = pcl::io::savePCDFileBinary(req.destination, *filtered);
    }else{
      ret = pcl::io::savePCDFileBinary(req.destination, *cloud);
    }
    res.success = ret == 0;
    return true;
  }

  void LaserScanMapping::pointcloud_to_occupancy_grid( const std::vector<KeyFrameSnapshot::Ptr>& keyframes, const pcl::PointCloud<PointT>& cloud){
    double resolution = nh_private_.param<double>("occupancy_grid_resolution", 0.025);
    pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>());
    // pcl::VoxelGrid<PointT> sor; 
	  // sor.setInputCloud(cloud);          
	  // sor.setLeafSize(resolution, resolution, resolution);
	  // sor.filter(*cloud_filtered);
    *cloud_filtered = *cloud;
    std::cout << "finished save size " << cloud_filtered->points.size() << std::endl;
    PointT min, max;
    pcl::getMinMax3D(*cloud_filtered, min, max);
    double origin_x = min.x;
    double origin_y = min.y;
    int width = (max.x - origin_x)/ resolution;
    int height = (max.y - origin_y)/ resolution;
    if (width <=0 || height<=0)
    {
      RCLCPP_WARN(node->get_logger(),"MAP SIZE 0");
      return;
    }
    
    nav_msgs::msg::OccupancyGrid map;
    map.header.frame_id = map_frame_id;
    map.header.stamp = rclcpp::Clock().now();
    map.info.width = width;
    map.info.height = height;
    map.info.resolution = resolution;
    map.info.origin.position.x = origin_x;
    map.info.origin.position.y = origin_y;
    map.info.origin.orientation.w = 1.0;
    map.data.resize(width * height);
    //转图片处理
    if (nh_private_.param<bool>("enable_no_information", true))
    {
      bool enable_occupancy_grid_filtered = nh_private_.param<bool>("enable_occupancy_grid_filtered", true);

      cv::Mat costmap_img(height , width, CV_8UC1, cv::Scalar::all(100));
      //1. 点云转激光
      sensor_msgs::msg::LaserScan output;
      output.header.stamp = rclcpp::Clock().now();
      output.header.frame_id = "map";
      output.angle_min = -M_PI;
      output.angle_max = M_PI;
      output.angle_increment = M_PI / 720.0;
      output.time_increment = 0.0;
      output.scan_time = 1.0 / 30.0;
      output.range_min = 0.0;
      output.range_max = 100.0;
      // determine amount of rays to create
      uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment);
      output.ranges.assign(ranges_size, 0.05);

      PointT center;
      center.x = min.x + (max.x - min.x ) / 2.0;
      center.y = min.y + (max.y - min.y ) / 2.0;

      for (int i = 0; i < cloud_filtered->points.size(); i++)
      {
        PointT point = cloud_filtered->points[i];
        double range = hypot(point.x - center.x, point.y - center.y);
        double angle = atan2(point.y - center.y, point.x - center.x);

        int index = (angle - output.angle_min) / output.angle_increment;
        if (range > output.ranges[index])
        {
          output.ranges[index] = range;
        }
      }
      // debug_scan_pub_->publish(output);
      
      int radius = (int)(5.0 / resolution);
      //轨迹画圈
      for(const auto& keyframe : keyframes) {
        Eigen::Matrix4f pose = keyframe->pose.matrix().cast<float>();
        // printf("t = < %6.3f, %6.3f, %6.3f >\n\n", pose(0, 3), pose(1, 3), pose(2, 3));
        int mx = (int)((pose(0, 3) - min.x)/ resolution + 0.5);
        int my = (int)((pose(1, 3) - min.y)/ resolution + 0.5);
        cv::Point point = cv::Point(mx, costmap_img.rows - 1 - my);
        cv::circle(costmap_img, point, radius, cv::Scalar(255), -1, cv::LINE_AA, 0);
      }

      //2. 激光转图片
      double angle = output.angle_min;
      for (int i = 0; i < ranges_size; i++, angle += output.angle_increment)
      {
        double range = output.ranges[i];
        // if (range < 0.1)
        // {
        //   if (i > 0 && i < ranges_size)
        //   {
        //     range = std::max(output.ranges[i-1], output.ranges[i+1]);
        //   }
        // }
        double wx = range * cos(angle);
        double wy = range * sin(angle);
        int mx = (int)((wx + center.x - min.x)/ resolution + 0.5);
        int my = (int)((wy + center.y - min.y)/ resolution + 0.5);
        cv::Point laser_point = cv::Point(mx, costmap_img.rows - 1 - my);
        if (laser_point.x > costmap_img.cols || laser_point.y > costmap_img.rows)
        {
          laser_point.x = std::min(laser_point.x, costmap_img.cols);
          laser_point.y = std::min(laser_point.y, costmap_img.rows);
        }
        
        if (laser_point.x < 0 || laser_point.y < 0)
        {
          laser_point.x = std::max(laser_point.x, 0);
          laser_point.y = std::max(laser_point.y, 0);
        }

        cv::line(costmap_img, cv::Point(width / 2, height / 2), laser_point, cv::Scalar(255), 5);
      }

      for (int i = 0; i < cloud_filtered->points.size(); i++)
      {
        PointT obs_point = cloud_filtered->points[i];
        int obs_mx = (int)((obs_point.x - origin_x) / resolution + 0.5);
        int obs_my = (int)((obs_point.y - origin_y) / resolution + 0.5);
        cv::Point obs_point_im = cv::Point(obs_mx, costmap_img.rows - 1 - obs_my);
        if (obs_point_im.x > costmap_img.cols || obs_point_im.y > costmap_img.rows)
        {
          obs_point_im.x = std::min(obs_point_im.x, costmap_img.cols);
          obs_point_im.y = std::min(obs_point_im.y, costmap_img.rows);
        }

        if (obs_point_im.x < 0 || obs_point_im.y < 0)
        {
          obs_point_im.x = std::max(obs_point_im.x, 0);
          obs_point_im.y = std::max(obs_point_im.y, 0);
        }
        costmap_img.at<uchar>(obs_point_im.y, obs_point_im.x) = 0;
      }
      
      std::vector<cv::Point> neighbor_points = {cv::Point(-1, -1), cv::Point(0, -1), cv::Point(1, -1), cv::Point(1, 0), cv::Point(1, 1), cv::Point(0, 1), cv::Point(-1, 1), cv::Point(-1, 0)};

      //3. image to OccupancyGrid
      for (int i = 0; i < height; i++)
      {
        for (int j = 0; j < width; j++)
        {
          if (costmap_img.at<uchar>(costmap_img.rows - 1 - i, j) == 0)
          {
            if(enable_occupancy_grid_filtered)
            {
              //检查8个方向的点是否都不是障碍物
              int cout  = 0;
              cv::Point current = cv::Point(j, costmap_img.rows - 1 - i);
              int start_index = -1;
              for (int k = 0; k < neighbor_points.size(); k++)
              {
                cv::Point tmppoint = current + neighbor_points[k];
                if (tmppoint.x > 0 && tmppoint.y > 0 && tmppoint.x < costmap_img.cols && tmppoint.y < costmap_img.rows){
                  if (costmap_img.at<uchar>(tmppoint) == 0){
                    cout++;
                    if (start_index == -1 || k!= start_index+1)
                    {
                      start_index = k;
                    }

                    if (cout >3)
                    {
                      break;
                    }
                  }
                }
              }
              map.data[i*width+j] = cout > 0 ? LETHAL_OBSTACLE : FREE_SPACE;
              
              if (cout == 3 && (start_index == 0 || start_index == 2 || start_index == 4 || start_index == 6))
              {
                map.data[i*width+j] = FREE_SPACE;
              }

              if (map.data[i*width+j] == FREE_SPACE)
              {
                costmap_img.at<uchar>(costmap_img.rows - 1 - i, j) = 0;
              }
            }else
            {
              map.data[i*width+j] = LETHAL_OBSTACLE;
            }
            
          }
          else if (costmap_img.at<uchar>(costmap_img.rows - 1 - i, j) == 255)
          {
            map.data[i*width+j] = FREE_SPACE;
          }else{
            map.data[i*width+j] = NO_INFORMATION;
          }
        }
      }
    }else{
      for (size_t i = 0; i < width * height; i++)
      {
        map.data[i] = 0;
      }      
      for (int i = 0; i < cloud_filtered->points.size(); i++)
      {
        PointT point = cloud_filtered->points[i];

        int x = (point.x - origin_x ) / resolution;
        int y = (point.y - origin_y) / resolution;

        int index = x + y * width;
        if (index >=0 && index < width * height)
        {
          map.data[index] = 100;
        }
      }
    }
    map_grid_pub_->publish(map);
  }

  /**
   * @description: 远处激光点位置不准确,建图容易重影
   * @param:  
   * @return:   
   */
  pcl::PointCloud<LaserScanMapping::PointT> LaserScanMapping::distance_filter(const pcl::PointCloud<PointT>& cloud) const {
    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    filtered->reserve(cloud->size());
    
    std::copy_if(cloud->begin(), cloud->end(), std::back_inserter(filtered->points), [&](const PointT& p) {
      double d = p.getVector3fMap().norm();
      return d > distance_near_thresh && d < distance_far_thresh;
    });
    filtered->width = filtered->size();
    filtered->height = 1;
    filtered->is_dense = false;
    filtered->header = cloud->header;
    return filtered;
  }
} // namespace laser_scan_graph_slam


